//////////////////////////////////////////////////////////////////////////////////////////////////////////
//		            BulletModifyHinge
//////////////////////////////////////////////////////////////////////////////////////////////////////////
#pragma warning (disable: 4819)
#include "CKAll.h"
#include "BulletManager.h"

#define INPUT_PARAMENTER_CONSTRAINTID				0
#define INPUT_PARAMENTER_LOWERLIMIT					1
#define INPUT_PARAMENTER_UPPERLIMIT					2
#define INPUT_PARAMENTER_VELOCITY						3
#define INPUT_PARAMENTER_MAXIMOTORIMPULSE	4
#define INPUT_PARAMENTER_KEEPANGLE					5
#define INPUT_PARAMENTER_SOFTNESS						6
#define INPUT_PARAMENTER_BIASFACTOR					7


CKObjectDeclaration	*FillBehaviorBulletModifyHingeDecl();
CKERROR CreateBulletModifyHingeProto(CKBehaviorPrototype **);
int BulletModifyHinge(const CKBehaviorContext& BehContext);

CKObjectDeclaration	*FillBehaviorBulletModifyHingeDecl()
{
	CKObjectDeclaration *od = CreateCKObjectDeclaration("BulletModifyHinge");	
	
	od->SetType(CKDLL_BEHAVIORPROTOTYPE);
	od->SetVersion(0x00000001);
	od->SetCreationFunction(CreateBulletModifyHingeProto);
	od->SetDescription("Bullet Modify Hinge ");
	od->SetCategory("Bullet/Constraint");
	od->SetGuid(CKGUID(0x41556d8d,0x760046ef));
	od->SetAuthorGuid(CKGUID(0x457d2f46,0x3ee809a8));
	od->SetAuthorName("gjfeilipu");
	od->SetCompatibleClassId(CKCID_BEOBJECT);

	return od;
}

CKERROR CreateBulletModifyHingeProto(CKBehaviorPrototype** pproto)
{
	CKBehaviorPrototype *proto = CreateCKBehaviorPrototype("BulletModifyHinge");
	if (!proto) {
		return CKERR_OUTOFMEMORY;
	}

//[!output INPUT_DECL]
//[!output OUTPUT_DECL]
//[!output INPUTP_DECL]
//[!output OUTPUTP_DECL]
	proto->DeclareInput("On");
	proto->DeclareInput("Off");
	proto->DeclareOutput("On Out");
	proto->DeclareOutput("Off Out");

	proto->DeclareInParameter("Constraint ID",CKPGUID_INT,"-1");
	proto->DeclareInParameter("LowerLimit",CKPGUID_FLOAT,"0");
	proto->DeclareInParameter("UpperLimit",CKPGUID_FLOAT,"0");
	proto->DeclareInParameter("Velocity",CKPGUID_FLOAT,"0.1");
	proto->DeclareInParameter("MaxMotorImpulse",CKPGUID_FLOAT,"10");
	proto->DeclareInParameter("KeepAngle",CKPGUID_BOOL);
	proto->DeclareInParameter("Softness",CKPGUID_FLOAT,"0.9");
	proto->DeclareInParameter("BiasFactor",CKPGUID_FLOAT,"0.3");

	proto->DeclareOutParameter("Constraint ID",CKPGUID_INT,"-1");
	proto->DeclareOutParameter("Angle",CKPGUID_FLOAT,"0");

//----	Local Parameters Declaration

//----	Settings Declaration
//[!output BB_FLAGS]
//[!output BB_CALLBACKREG]
	proto->SetFlags(CK_BEHAVIORPROTOTYPE_NORMAL);
	proto->SetFunction(BulletModifyHinge);

	*pproto = proto;
	return CK_OK;
}

int BulletModifyHinge(const CKBehaviorContext& BehContext)
{
	CKBehavior* beh = BehContext.Behavior;
	CKContext* cex = beh->GetCKContext();
	
	btHingeConstraint* hingeConstraint = NULL;
	BulletManager* bManager =  (BulletManager*)cex->GetManagerByGuid(BulletManagerGUID);
	if(bManager)
	{
		if(beh->IsInputActive(1))
		{		
			bool bStayAngle = false;
			int ID = -1;
			float softness = 0;
			float biasFactor = 0;
			beh->GetInputParameterValue(INPUT_PARAMENTER_KEEPANGLE, &bStayAngle);
			beh->GetInputParameterValue(INPUT_PARAMENTER_CONSTRAINTID, &ID);
			beh->GetInputParameterValue(INPUT_PARAMENTER_SOFTNESS, &softness);
			beh->GetInputParameterValue(INPUT_PARAMENTER_BIASFACTOR, &biasFactor);
			hingeConstraint = (btHingeConstraint*)bManager->getConstraint(ID);
			if(hingeConstraint)
			{
				btScalar hingeAngle, lowLim, hiLim;
				if(bStayAngle)
				{					
					hingeAngle = hingeConstraint->getHingeAngle();
					lowLim = hingeConstraint->getLowerLimit();
					hiLim = hingeConstraint->getUpperLimit();
					if(hiLim < lowLim)
					{
						hingeConstraint->setLimit(hingeAngle, hingeAngle, softness, biasFactor);
					}else 	if(hingeAngle < lowLim)
					{
						hingeConstraint->setLimit(lowLim, lowLim, softness, biasFactor);
					}else if(hingeAngle > hiLim)
					{
						hingeConstraint->setLimit(hiLim, hiLim, softness, biasFactor);
					}else
					{
						hingeConstraint->setLimit(hingeAngle, hingeAngle, softness, biasFactor);
					}
				}
				hingeConstraint->enableAngularMotor(false, 0, 0);
				beh->SetOutputParameterValue(1, &hingeAngle);
			}
			beh->SetOutputParameterValue(0, &ID);
			beh->ActivateInput(1,FALSE);
			beh->ActivateOutput(1,TRUE);
			return CKBR_OK;
		}else 	if(beh->IsInputActive(0))
		{
			beh->ActivateInput(0,FALSE);
			beh->ActivateOutput(0,TRUE);
		}

		//bManager = (BulletManager*)cex->GetManagerByGuid(BulletManagerGUID);//->addRigidBody(btRigidBody *body);
		//if(bManager)
		int ID = -1;
		beh->GetInputParameterValue(INPUT_PARAMENTER_CONSTRAINTID, &ID);
		hingeConstraint = (btHingeConstraint*)bManager->getConstraint(ID);
		if(hingeConstraint)
		{
			btScalar btLowerLimit, btUpperLimit, btVelocity, btMaxMotorImpulse;
			float softness = 0;
			float biasFactor = 0;
			beh->GetInputParameterValue(INPUT_PARAMENTER_LOWERLIMIT, &btLowerLimit);
			beh->GetInputParameterValue(INPUT_PARAMENTER_UPPERLIMIT, &btUpperLimit);
			beh->GetInputParameterValue(INPUT_PARAMENTER_VELOCITY, &btVelocity);
			beh->GetInputParameterValue(INPUT_PARAMENTER_MAXIMOTORIMPULSE, &btMaxMotorImpulse);
			beh->GetInputParameterValue(INPUT_PARAMENTER_SOFTNESS, &softness);
			beh->GetInputParameterValue(INPUT_PARAMENTER_BIASFACTOR, &biasFactor);

			hingeConstraint->setLimit(btLowerLimit, btUpperLimit, softness, biasFactor);
			hingeConstraint->enableAngularMotor(true, btVelocity, btMaxMotorImpulse);
			
			btScalar hingeAngle = hingeConstraint->getHingeAngle();
			beh->SetOutputParameterValue(0, &ID);
			beh->SetOutputParameterValue(1, &hingeAngle);
		}
		return CKBR_ACTIVATENEXTFRAME;
	}
	return CKBR_OK;
}